//
// Created by zhangzhen on 2020/11/12.
//
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h> // for tf::getPrefixParam()
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>

std::string frame_id_;
std::string footprint_frame_id_;
std::string base_frame_id_;
double smooth_period;

ros::Publisher odom_pub_, odom_smooth_pub_;
tf::TransformBroadcaster *transform_broadcaster_;

std::deque<nav_msgs::Odometry> odometry_deque_;

void addTransform(std::vector<geometry_msgs::TransformStamped>& transforms, const tf::StampedTransform& tf)
{
  transforms.push_back(geometry_msgs::TransformStamped());
  tf::transformStampedTFToMsg(tf, transforms.back());
}

void groundTruthCallback(nav_msgs::OdometryConstPtr msg) {
  std::vector<geometry_msgs::TransformStamped> transforms;

  static tf::StampedTransform tf_last;
  tf::StampedTransform tf;
  tf.stamp_ = msg->header.stamp;
  double time_duration = tf.stamp_.toSec() - tf_last.stamp_.toSec();
//  static double stamp_last = 0.0;
//  double stamp_now = ros::Time::now().toSec();
//  time_duration = stamp_now - stamp_last;
//  stamp_last = stamp_now;
  ROS_INFO("odometry duration: %f", time_duration);

  tf::Quaternion orientation;
  tf::quaternionMsgToTF(msg->pose.pose.orientation, orientation);
  tfScalar yaw, pitch, roll;
  tf::Matrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
  tf::Point position;
  tf::pointMsgToTF(msg->pose.pose.position, position);
  tf.setOrigin(position);
  tf.setRotation(orientation);

  tf::StampedTransform tf_relative;
  tf_relative.mult(tf_last.inverse(), tf);
  tf_last = tf;

  // publish odometry topic
  nav_msgs::Odometry odom_msg;
  odom_msg.header = msg->header;
  odom_msg.pose = msg->pose;
  odom_msg.twist.twist.linear.x = tf_relative.getOrigin().getX() / time_duration;
  odom_msg.twist.twist.linear.y = tf_relative.getOrigin().getY() / time_duration;
  odom_msg.twist.twist.linear.z = tf_relative.getOrigin().getZ() / time_duration;
  double r_delta[3];
  tf_relative.getBasis().getRPY(r_delta[0], r_delta[1], r_delta[2]);
  odom_msg.twist.twist.angular.x = r_delta[0] / time_duration;
  odom_msg.twist.twist.angular.y = r_delta[1] / time_duration;
  odom_msg.twist.twist.angular.z = r_delta[2] / time_duration;
  odom_pub_.publish(odom_msg);

  // publish smooth twist
  odometry_deque_.push_back(odom_msg);
  if (!odometry_deque_.empty() &&
      odometry_deque_.back().header.stamp.toSec() - odometry_deque_.front().header.stamp.toSec() > smooth_period) {
    odometry_deque_.pop_front();
  }
  geometry_msgs::Twist mean_twist, sum_twist;
  int twist_size = odometry_deque_.size();
  for (auto &it : odometry_deque_) {
    sum_twist.linear.x += it.twist.twist.linear.x;
    sum_twist.linear.y += it.twist.twist.linear.y;
    sum_twist.linear.z += it.twist.twist.linear.z;
    sum_twist.angular.x += it.twist.twist.angular.x;
    sum_twist.angular.y += it.twist.twist.angular.y;
    sum_twist.angular.z += it.twist.twist.angular.z;
  }
  mean_twist.linear.x = sum_twist.linear.x / twist_size;
  mean_twist.linear.y = sum_twist.linear.y / twist_size;
  mean_twist.linear.z = sum_twist.linear.z / twist_size;
  mean_twist.angular.x = sum_twist.angular.x / twist_size;
  mean_twist.angular.y = sum_twist.angular.y / twist_size;
  mean_twist.angular.z = sum_twist.angular.z / twist_size;

  odom_msg.twist.twist = mean_twist;
  odom_smooth_pub_.publish(odom_msg);

  // footprint intermediate transform (x,y,yaw)
  tf.frame_id_ = frame_id_;
  tf.child_frame_id_ = footprint_frame_id_;
  tf.setOrigin(tf::Vector3(position.x(), position.y(), 0.0));
  tf.setRotation(tf::createQuaternionFromRPY(0.0, 0.0, yaw));
  addTransform(transforms, tf);

  // base_link transform (x, y, z, roll, pitch, yaw)
  tf.frame_id_ = footprint_frame_id_;
  tf.child_frame_id_ = base_frame_id_;
  tf.setOrigin(tf::Vector3(0.0, 0.0, position.z()));
  tf.setRotation(tf::createQuaternionFromRPY(roll, pitch, 0.0));
  addTransform(transforms, tf);

  transform_broadcaster_->sendTransform(transforms);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "fake_odometry");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");

  std::string odometry_topic;
  std::string ground_truth_topic;

  priv_nh.param<std::string>("odometry_topic", odometry_topic, "/odom");
  priv_nh.param<std::string>("ground_truth_topic", ground_truth_topic, "/odom/ground_truth");
  priv_nh.param<double>("smooth_period", smooth_period, 0.5);
  priv_nh.getParam("frame_id", frame_id_);
  priv_nh.getParam("footprint_frame_id", footprint_frame_id_);
  priv_nh.getParam("base_frame_id", base_frame_id_);

  ros::Subscriber sub1 = nh.subscribe<nav_msgs::Odometry>(ground_truth_topic, 10, &groundTruthCallback);
  odom_pub_ = nh.advertise<nav_msgs::Odometry>(odometry_topic, 10);
  odom_smooth_pub_ = nh.advertise<nav_msgs::Odometry>(odometry_topic + "_smooth", 10);

  transform_broadcaster_ = new tf::TransformBroadcaster;

  ros::spin();
  delete transform_broadcaster_;
  return 0;
}